#include "Listener.h"
#include "Tokamak.h"
#pragma comment (lib, "Libs/Tokamak/lib/TokamakDLL.lib")
//#pragma comment (lib, "Libs/tokamak.lib")
#include "Util.h"
#include "App.h"
#include "Sprite.h"
#include "PhysicsSpriteController.h"
#include "GameMessages.h"
#include "NetworkDGram.h"

//This distribution include Tokamak in DLL format.	 To use TokamakDll.dll, include the header file
//Tokamak.h, and define TOKAMAK_USE_DLL in your compiler setting, and link TokamakDll.lib.

neSimulator				*simulator=null;
std::vector<Sprite*>	physicsSprites;


SpriteDef*						playerSpriteDef=null;
Sprite*							playerSprite=null;
extern	Sprite*					playerSprite;
s32								gRagdollStackTop=0;

void	CollisionCallback(neCollisionInfo & collisionInfo);

void	PhysicsSpriteController::render(Sprite*	spr){
	//trail.markForRendering();
	overlay.markForRendering();
}

void	AnimatedSpriteController::update(Sprite*	spr){
	//trail.markForRendering();
}
void	AnimatedSpriteController::render(Sprite*	spr){
	//trail.markForRendering();
}

void	PhysicsSpriteController::childCollision(neCollisionInfo & collisionInfo){
	if((collisionInfo.typeA==NE_RIGID_BODY)&&(collisionInfo.typeA==collisionInfo.typeB)){
		//timeToRecover=1.0f;
		frameValid=false;
	}
}

void	PhysicsSpriteController::collision(neCollisionInfo & collisionInfo){
	if(parent){
		parent->childCollision(collisionInfo);
	}
	neByte* bdy=(neByte*)body;
	if(collisionInfo.typeB==NE_RIGID_BODY&&collisionInfo.typeA==NE_RIGID_BODY&&(collisionInfo.bodyA==bdy||collisionInfo.bodyB==bdy)){

#if 0
		char*	strs[]={	//Dump params of walk controller
			"stanceWidth",
			"stepRearShift",
			"strideLength",
			"calfCenterHeight",
			"armRearShift",
			"armSwingWidth",
			"armSwingLength",
			"walkSpeed",
			"stepMag",
			"stepHeight",
			"angularDamping",
			"linearDamping",
			"armPhaseShift"
		};
		logging(true);
		logstr("------walk params------\n");
		for(int i=0;i<array_size(strs);i++){
			logstr(format("		%s=%4.4ff;\n",strs[i],*((&stanceWidth)+i)));
		}
		logging(false);
#endif

		timeToRecover=1.0f;
		hitpoints-=0.1f;
		if(hitpoints<0.0f)hitpoints=0.0f;
	}else if((collisionInfo.typeB==NE_RIGID_BODY&&
				(collisionInfo.typeA==NE_TERRAIN||collisionInfo.typeA==NE_ANIMATED_BODY))
			||(collisionInfo.typeA==NE_RIGID_BODY&&
			(collisionInfo.typeB==NE_TERRAIN||collisionInfo.typeB==NE_ANIMATED_BODY))){
		hadTerrainContact=1.0f;
		lastContactPoint=(vec3&)collisionInfo.worldContactPointA;
	}
}

void PhysicsSpriteController::acquirePoint(vec3	pt,float	force,float	maxdist){
	vec3 cpt=((vec3&)body->GetPos());
	vec3 dlt=pt-cpt;
	vec3 vel=((vec3&)body->GetVelocity());
	float dist=length(dlt);
	dlt=dlt/dist;
	if(dist>maxdist)dist=maxdist;
	dlt=dlt*force*dist/maxdist;

	if(dot(vel,dlt)<0.0f){
		vel*=0.95f;//Damp oscillation
	}
	body->SetVelocity(((neV3&)(vel+dlt)));
	//body->SetPos(((neV3&)pt));
}

void PhysicsSpriteController::standController(float strength){
	PhysicsSpriteController&	head=*children[1];
	PhysicsSpriteController&	torso=*children[0];
	PhysicsSpriteController&	larm=*children[4];
	PhysicsSpriteController&	rarm=*children[5];
	PhysicsSpriteController&	lcalf=*children[8];
	PhysicsSpriteController&	rcalf=*children[9];
	
	bool	rplanted=false;
	bool	lplanted=false;
	float	footInterval=gApp->simTime*0.6f*walkSpeed;

	float	footPhase=sin(footInterval*PI);
	footInterval=footInterval-(int)footInterval;
	bool	leftFoot=true;
	
	vec3 npos=((vec3&)head.body->GetPos());
	vec3 headCenter=npos;
	vec3 calfCenter=(vec3&)((lcalf.body->GetPos()+rcalf.body->GetPos())*0.5f);


	if(footPhase<0.0f){
		footPhase*=-1.0f;
		leftFoot=false;
	}
	//return invisibleBeatdownController(strength);


	//torso.body->SetVelocity(torso.body->GetVelocity()*0.9f);
	
	
	if(lcalf.hadTerrainContact>0.1f){
		//if(lcalf.body->GetPos().v[2]<1.0f){	//Damp the feet...
		if(lcalf.body->GetPos().n.Z<(framePosition+(calfCenter*0.5f)).z){frameValid=false;}//timeToRecover=1.0f;}
		if(!leftFoot)
			lcalf.body->SetVelocity(lcalf.body->GetVelocity()*(1.0f-strength));
			lplanted=true;

		//}
		lcalf.hadTerrainContact*=0.9f;
	}
	if(rcalf.hadTerrainContact>0.1f){
		if(rcalf.body->GetPos().n.Z<(framePosition+(calfCenter*0.5f)).z){frameValid=false;}//timeToRecover=1.0f;}
		if(leftFoot) //Damp the feet..
			rcalf.body->SetVelocity(rcalf.body->GetVelocity()*(1.0f-strength));
		//}
			rplanted=true;
		rcalf.hadTerrainContact*=0.9f;
	}

	if(!(rplanted||lplanted)){
		//falling
		isFalling=true;
		//frameValid=false;
		//return;
	}

	if(frameValid){
		if(length((npos-framePosition).xy())>2.5f){
			frameValid=false;
		}
	}
	
	if((rplanted||lplanted)&&(!frameValid)&&(timeToRecover<0.01f)){
		//Create our frame at our landing point..
		framePosition=vec3(0,0,0);
		int	 rc=0;
		if(lplanted){framePosition+=lcalf.lastContactPoint;rc++;};
		if(rplanted){framePosition+=rcalf.lastContactPoint;rc++;};
		framePosition/=(float)rc;
		frameRotation=(mat3&)torso.body->GetRotationM3();

		frameRotation=rotateZ3(frand()*PI*2.0f);//identity3();
		frameValid=true;

		for(unsigned int t=0;t<this->children.size();t++){
			children[t]->body->SetAngularDamping(angularDamping);
			children[t]->body->SetLinearDamping(linearDamping);
		}
	}
	//if(frameValid)
	{
		gApp->debugTransform(framePosition,frameRotation,1.5f,color4::black);
	}

	calfCenter.z=min(lcalf.body->GetPos().n.Z,rcalf.body->GetPos().n.Z);
	vec3 cogground=vec3(headCenter.xy(),calfCenter.z);
	if(frameValid)
	{
		npos=framePosition;
		npos.z+=3.0f;//5.0 Maintain head at fixed distance above calves. 4.7 makes them dance..

	}else{
		npos = calfCenter;	//point between both calves
		npos.z=calfCenter.z+2.5f;//4.5 Maintain head at fixed distance above calves. 4.7 makes them dance..
	}

	float ackStr=3.0f;//5.8f;//3.8

	ackStr+=footPhase*-1.0f;


	gApp->debugMarker(npos);
	acquirePoint(npos,ackStr*strength,1.3f);//6.5


	
	//raise/lower leg.
	vec3 lground=vec3(((vec3&)lcalf.body->GetPos()).xy(),calfCenter.z+(footPhase*0.5f));
	vec3 rground=vec3(((vec3&)rcalf.body->GetPos()).xy(),calfCenter.z+(footPhase*0.5f));

	
	//vec3 torsoRight=(vec3&)(torso.body->GetTransform().rot.M[0]);
	//vec3 torsoFore=(vec3&)(torso.body->GetTransform().rot.M[2]);
	//frameRotation=identity3();
	vec3 frameRight=(vec3&)frameRotation.getRow(0);
	vec3 frameFore=(vec3&)frameRotation.getRow(1);
	vec3 frameUp=(vec3&)frameRotation.getRow(2);
	vec3 torsoGround=vec3(((vec3&)(torso.body->GetPos())).xy(),calfCenter.z);
	
	if(frameValid){
		framePosition+=frameFore*-0.008f*walkSpeed;
	}else
		return;

	vec3	frmwalkoffset=framePosition+(frameFore*footInterval);

	//for(int t=0;t<100;t++){
	//	gApp->debugLine(vec3(frand()*10,frand()*10,frand()*10),vec3(frand()*10,frand()*10,frand()*10));
	//}
	if(leftFoot){
		lground=(framePosition-(frameRight*stanceWidth))+(frameUp*calfCenterHeight);

		lground.z+=footPhase*stepHeight;
		lground-=(footInterval-stepRearShift)*frameFore*strideLength*stepMag;
		lcalf.acquirePoint(lground,1.5f*strength,1.0f);
		
		gApp->debugTransform(lground,frameRotation,0.2f);


	}else {
		rground=(framePosition+(frameRight*stanceWidth))+(frameUp*calfCenterHeight);
		rground.z+=footPhase*stepHeight;
		rground-=(footInterval-stepRearShift)*frameFore*strideLength*stepMag;
		rcalf.acquirePoint(rground,1.5f*strength,1.0f);
		
		gApp->debugTransform(rground,frameRotation,0.2f);
	}
	float	armPhase=(footInterval-armPhaseShift)*armSwingLength;
	vec3	armSwing;

	if(leftFoot)armSwing=frameFore*(armPhase*-1.0f);
	else armSwing=frameFore*armPhase;

	vec3	larmtarg=(framePosition+(frameUp*1.5f))-(frameRight*armSwingWidth)-armSwing+(armRearShift*frameFore);
	vec3	rarmtarg=(framePosition+(frameUp*1.5f))+(frameRight*armSwingWidth)+armSwing+(armRearShift*frameFore);
	//gApp->debugTransform(larmtarg,frameRotation,0.1f);
	//gApp->debugTransform(rarmtarg,frameRotation,0.1f);
		rarm.acquirePoint(rarmtarg,3.5f*strength,6.0f);
		larm.acquirePoint(larmtarg,3.5f*strength,6.0f);
}


void PhysicsSpriteController::invisibleBeatdownController(float strength){
	PhysicsSpriteController&	lcalf=*children[8];
	PhysicsSpriteController&	rcalf=*children[9];
	PhysicsSpriteController&	head=*children[1];
	PhysicsSpriteController&	torso=*children[0];
	bool	rplanted=false;
	bool	lplanted=false;
	//torso.body->SetVelocity(torso.body->GetVelocity()*0.9f);
	
	
	if(lcalf.hadTerrainContact>0.1f){
		//if(lcalf.body->GetPos().v[2]<1.0f){	//Damp the feet...
			lcalf.body->SetVelocity(lcalf.body->GetVelocity()*(1.0f-strength));
			lplanted=true;

		//}
		lcalf.hadTerrainContact*=0.7f;
	}
	if(rcalf.hadTerrainContact>0.1f){
		//if(rcalf.body->GetPos().v[2]<1.0f){ //Damp the feet..
			rcalf.body->SetVelocity(rcalf.body->GetVelocity()*(1.0f-strength));
		//}
			rplanted=true;
		rcalf.hadTerrainContact*=0.7f;
	}

	if(!(rplanted||lplanted)){
		//falling
		isFalling=true;
		//frameValid=false;
		return;
	}
	vec3 npos=((vec3&)head.body->GetPos());
	vec3 headCenter=npos;
	vec3 calfCenter=(vec3&)((lcalf.body->GetPos()+rcalf.body->GetPos())*0.5f);

	
	if(rplanted&&lplanted&&(!frameValid)){
		//Create our frame at our landing point..
		framePosition=(lcalf.lastContactPoint+rcalf.lastContactPoint)*0.5f;
		frameRotation=rotateZ3(frand()*PI*2.0f);
		frameValid=true;

	}
	if(frameValid){
	//	gApp->debugMarker(framePosition);
	}

	calfCenter.z=min(lcalf.body->GetPos().n.Z,rcalf.body->GetPos().n.Z);
	vec3 cogground=vec3(headCenter.xy(),calfCenter.z);
	if(frameValid){
		npos=framePosition;
		npos.z+=6.0f;//Maintain head at fixed distance above calves. 4.7 makes them dance..
	}else{
		npos = calfCenter;	//point between both calves
		npos.z=calfCenter.z+3.2f;//Maintain head at fixed distance above calves. 4.7 makes them dance..
	}
	
	//gApp->debugMarker(npos);

	acquirePoint(npos,3.5f*strength,3.5f);


	
	//raise/lower leg.
	float	footInterval=gApp->simTime*0.33f;

	float	footPhase=sin(footInterval*PI);
	footInterval=footInterval-(int)footInterval;
	bool	leftFoot=true;

	if(footPhase<0.0f){
		footPhase*=-1.0f;
		leftFoot=false;
	}
	vec3 lground=vec3(((vec3&)lcalf.body->GetPos()).xy(),calfCenter.z+(footPhase*0.5f));
	vec3 rground=vec3(((vec3&)rcalf.body->GetPos()).xy(),calfCenter.z+(footPhase*0.5f));

	
	vec3 torsoRight=(vec3&)(torso.body->GetTransform().rot.M[0]);
	vec3 torsoFore=(vec3&)(torso.body->GetTransform().rot.M[2]);
	vec3 torsoGround=vec3(((vec3&)(torso.body->GetPos())).xy(),calfCenter.z);
	const float stepMag=0.6f;

	//for(int t=0;t<100;t++){
	//	gApp->debugLine(vec3(frand()*10,frand()*10,frand()*10),vec3(frand()*10,frand()*10,frand()*10));
	//}
	if(leftFoot){
		lground=torsoGround-(torsoRight*0.20f);

		lground.z+=footPhase*stepMag;
		lground+=(footInterval-0.25f)*torsoFore*1.5f*stepMag;
		lcalf.acquirePoint(lground,1.5f*strength,2.0f);

	//	gApp->debugMarker(lground);


	}else{
		rground=torsoGround+(torsoRight*0.20f);

		rground.z+=footPhase*stepMag;
		rground+=(footInterval-0.25f)*torsoFore*1.5f*stepMag;
		rcalf.acquirePoint(rground,1.5f*strength,2.0f);

	//	gApp->debugMarker(rground);
	}
}

void PhysicsSpriteController::distressedController(float strength){
	vec3 nvel=((vec3&)body->GetVelocity());
	float cforce=length(nvel.xy());
	float maxforce=1.2f;
	if(cforce<maxforce){
		float icforce=(cforce/maxforce);

		if(icforce>0.1f)icforce=0.1f;
		float cforce=1.0f-icforce;
		nvel.z=0.0f;
		nvel*=0.97f;
		vec3 npos=((vec3&)body->GetPos());
		npos.z=2.2f;

		npos=(npos*icforce)+(((vec3&)body->GetPos())*cforce);

		nvel=(nvel*icforce)+(((vec3&)body->GetVelocity())*cforce);

		body->SetVelocity(((neV3&)nvel));
		body->SetPos(((neV3&)npos));
	}
}

Sprite* makeRadDude(neSimulator*	simulator,neV3 position, mat3 rotation,s32 & index);

std::map<std::string,SpriteDef*>	gSpriteDefCache;

Sprite* makeSprite(char*	tilenm="tile_ball"){
	SpriteDef* spriteDefinition = null;
	Sprite* sprite=new Sprite();
	if(gSpriteDefCache.find(tilenm)==gSpriteDefCache.end()){
		//if(!gBallSpriteDefinition){
		spriteDefinition=new SpriteDef();
		gSpriteDefCache[tilenm]=spriteDefinition;
		if(!loadSpriteDef("testTiles",SPRITE_SHADER,spriteDefinition))//txonly
			return false;
		//	delete playerSpriteDef->models[0];
		spriteDefinition->models[0]=SpriteDef::getModel(tilenm);
		spriteDefinition->models[0]->makeDrawable(gApp->getRenderer());
	}else
		spriteDefinition=gSpriteDefCache.find(tilenm)->second;
	sprite->definition=spriteDefinition;
	return sprite;
}

Sprite* makeRigidBodySprite(neRigidBody*	nbody=0,char*	tilenm="tile_ball"){
	SpriteDef* spriteDefinition = null;
	neRigidBody* nrb=nbody;
	if(!nbody){
		nrb=simulator->CreateRigidBody();
		neGeometry*	geom=nrb->AddGeometry();
		float diam=1.0f;
		geom->SetSphereDiameter(diam);
		nrb->SetMass(0.5f);
		nrb->UpdateBoundingInfo();
		neV3 inertiaTensor;
		inertiaTensor = neSphereInertiaTensor(diam, 0.5f);
		nrb->SetInertiaTensor(inertiaTensor);
		//nrb->SetLinearDamping(0.01f);
		//nrb->SetAngularDamping(0.01f);
		nrb->SetSleepingParameter(1.0f);
		nrb->SetCollisionID(1);
	}
	Sprite* sprite=makeSprite(tilenm);
	sprite->controller=new PhysicsSpriteController(nrb);
	return sprite;
}

Sprite* makeAnimatedBodySprite(neAnimatedBody*	nbody=0,char*	tilenm="tile_wall"){
	SpriteDef* spriteDefinition = null;
	neAnimatedBody* nrb=nbody;
	if(!nbody){
		nrb=simulator->CreateAnimatedBody();
		neGeometry*	geom=nrb->AddGeometry();
		geom->SetBoxSize((neV3&)vec3(1000,1000,100));
		nrb->UpdateBoundingInfo();
		nrb->SetPos((neV3&)vec3(0,0,-50));
		//nrb->SetCollisionID(0);
	}
	Sprite* sprite=makeSprite(tilenm);
	sprite->controller=new AnimatedSpriteController(sprite,nrb);
	return sprite;
}


extern	bool	networkIsClient;
extern	bool	networkIsServer;
extern	bool	networkIsBroken;

void	PhysicsSpriteController::update(Sprite*	spr){
	neV3 v=body->GetPos();
	spr->position=vec3(v.n.X,v.n.Y,v.n.Z);
	neQ r=body->GetRotationQ();
	spr->quat.set(r.X,r.Y,r.Z,r.W*-1.0f);
	spr->quat.getMatrix(spr->transform,vec3(0,0,0));
	spr->transform.setTranslationVec(spr->position.x,spr->position.y,spr->position.z);

	static bool	 genocide = false;
	if(isBrain){
		if(timeToRecover>0.0f){
			timeToRecover-=0.0025f;
			overlay.text=format("recover:%4.4f",timeToRecover);

		}
		if(genocide){

		}else
		if(hitpoints>0.5f)
			standController((1.0f-timeToRecover));//Ease in the stand controller based on disorientation state
		else if(hitpoints>0.05f){
			hitpoints-=0.002f;
			distressedController((1.0f-timeToRecover));//Act distressed
		}else if(hitpoints>0.0f){
			hitpoints-=0.002f;
			standController((1.0f-timeToRecover));//Attempt one final standing before inevitable death
		}
	}

	if(gApp->camTargetID==spr->id){
		vec3	planeImpulse=vec3(gApp->controlImpulse.xy(),-2.0f/60.0f);	//Add some downward thrust to keep it on the ground..
		float cil=length(planeImpulse);
		planeImpulse*=20.0f;
		if(cil>8.0f)planeImpulse*=8.0f/cil;
		//normalize()*20.0f;
		float bv=body->GetVelocity().v[2]+planeImpulse.z*0.25f;
		if(jumpCountdown==0){
			if(gApp->keys[jumpKey[0]]){
				jumpCountdown=60;
				bv+=20.0f;
			}
		}else
			jumpCountdown--;
		//body->ApplyImpulse((neV3&)(planeImpulse*0.01f),v);
		vec3 nvel=(planeImpulse*0.25f)+(((vec3&)body->GetVelocity())*0.75f);
		nvel.z=bv;
		body->SetVelocity(((neV3&)nvel));

		if(gApp->lMouseButton||gApp->rMouseButton){
	
			if(networkIsClient){
				NetworkDGram	dgram;
				MakeNetworkDGram(dgram,control);
				dgram.control.controlBits=gApp->lMouseButton?1:2;
				dgram.control.targetSpriteID=spr->id;
				Listener::sendMessage("networkSendToServerTCP",(void*)&dgram);

			}else if(networkIsServer){
				//Headless server... do nothing...

			}else{
				//Not client or server.
				if(fireCountdown>0)fireCountdown--; else{
					fireCountdown=8;




					vec3 projectileExit=spr->position+(gApp->camModelView.getForwardVec()*1.5f)+(gApp->camModelView.getUpVec()*1.5f);
					Sprite*	nspr=null;
					if(gApp->lMouseButton){
						
						nspr=makeRigidBodySprite(0,"tile_egg");
						
						nspr->position=projectileExit;
						PhysicsSpriteController* ctl=((PhysicsSpriteController*)nspr->controller);
						neV3 projectileVel=(neV3&)(gApp->camModelView.getForwardVec()*30.0f)+ctl->body->GetVelocity();
						ctl->body->SetPos((neV3&)nspr->position);
						ctl->body->SetVelocity((neV3&)(gApp->camModelView.getForwardVec()*30.0f)+ctl->body->GetVelocity());

					}else{
						nspr=makeRadDude(simulator,(neV3&)projectileExit,rotateX3(PI*0.5f),gRagdollStackTop);
						PhysicsSpriteController* ctl=((PhysicsSpriteController*)nspr->controller);
						neV3 projectileVel=(neV3&)(gApp->camModelView.getForwardVec()*30.0f)+ctl->body->GetVelocity();//ctl->body->SetPos((neV3&)nspr->position);
						//ctl->body->SetVelocity();
					}
					//NetworkSendToClients(TCP,create);				
				}
			}
		}
	}
	//trail.update((vec3&)v);
	overlay.update((vec3&)v);
}

PhysicsSpriteController::~PhysicsSpriteController(){
}

float sfrand(float rng){
	return frrand(-rng,rng);
}

PhysicsSpriteController::PhysicsSpriteController(neRigidBody*	nbody){

	overlay.text="";//This is\nsome\ntest text.\n\n";

	body=nbody;
	body->SetUserData((u32)this);
	jumpCountdown=0;
	fireCountdown=0;
	isBrain=false;
	timeToRecover=0.0f;
	hitpoints=1.0f;
	parent=null;
	hadTerrainContact=0.0f;
	framePosition=(vec3&)nbody->GetPos();
	frameRotation=identity3();
	isFalling=true;
	frameValid=false;

	//G0
	stanceWidth=		0.2f	;
	stepRearShift=		1.0f;//1.5f	;
	strideLength=		1.5f	;
	calfCenterHeight=	0.2f	;
	armRearShift=		0.5f;//1.0f;//0.7f	;
	armSwingWidth=		0.5f	;
	armSwingLength=		0.6f	;
	walkSpeed=			2.0f	;
	stepMag=			0.4f	;
	stepHeight=			0.4f	;
	angularDamping=		0.005f	;
	linearDamping=		0.05f	;
	armPhaseShift=		0.25f	;
	/*
	//G1
	stanceWidth=0.1739f;
	stepRearShift=1.1988f;
	strideLength=1.4827f;
	calfCenterHeight=0.1506f;
	armRearShift=0.8087f;
	armSwingWidth=0.4555f;
	armSwingLength=0.7964f;
	walkSpeed=2.4101f;
	stepMag=0.5991f;
	stepHeight=0.4899f;
	angularDamping=0.0047f;
	linearDamping=0.0306f;
	armPhaseShift=0.1914f;
	*/

	float evoMag=0.5f;

	stanceWidth			+=sfrand(0.1f)*evoMag;
	stepRearShift		+=sfrand(0.3f)*evoMag;
	strideLength		+=sfrand(0.5f)*evoMag;
	calfCenterHeight	+=sfrand(0.05f)*evoMag;
	armRearShift		+=sfrand(0.3f)*evoMag;
	armSwingWidth		+=sfrand(0.2f)*evoMag;
	armSwingLength		+=sfrand(0.4f)*evoMag;
	walkSpeed			+=sfrand(1.0f)*evoMag;
	stepMag				+=sfrand(0.3f)*evoMag;
	stepHeight			+=sfrand(0.3f)*evoMag;
	angularDamping		+=sfrand(0.003f)*evoMag;
	linearDamping		+=sfrand(0.03f)*evoMag;
	armPhaseShift		+=sfrand(0.15f)*evoMag;

	linearDamping=abs(linearDamping);
	angularDamping=abs(angularDamping);

}

AnimatedSpriteController::~AnimatedSpriteController(){
}

AnimatedSpriteController::AnimatedSpriteController(Sprite* spr,neAnimatedBody*	nbody){
	body=nbody;
	body->SetUserData((u32)this);
	
	neV3 v=body->GetPos();
	spr->position=vec3(v.n.X,v.n.Y,v.n.Z);
	neQ r=body->GetRotationQ();
	spr->quat.set(r.X,r.Y,r.Z,r.W*-1.0f);
	spr->quat.getMatrix(spr->transform,vec3(0,0,0));
	spr->transform.setTranslationVec(spr->position.x,spr->position.y,spr->position.z);
}


void	CollisionCallback(neCollisionInfo & collisionInfo){
	if(collisionInfo.typeA==NE_RIGID_BODY){
		neRigidBody* bdy=(neRigidBody*)collisionInfo.bodyA;
		void*	ud=(void*)bdy->GetUserData();
		if(ud)((PhysicsSpriteController*)ud)->collision(collisionInfo);
	}
	if(collisionInfo.typeB==NE_RIGID_BODY){
		neRigidBody* bdy=(neRigidBody*)collisionInfo.bodyB;
		void*	ud=(void*)bdy->GetUserData();
		if(ud)((PhysicsSpriteController*)ud)->collision(collisionInfo);
	}
};


neAllocatorDefault allocator;

void	setTerrainMesh(std::vector<vec4>&	pvertices,std::vector<int>&	pindices){
	if(!simulator){
		MessageBox(0,"SetTerrain mesh called on null simulator.","error",0);
		return;
	}
	if(pvertices.size()==0||pindices.size()==0){
		MessageBox(0,"SetTerrain mesh called with no geometry defined.","error",0);
		return;
	}

#ifdef GROUND_QUAD
	float gpSz=10000.0f;
	vec3	verts[]={
		vec3(-gpSz,-gpSz,0),
		vec3( gpSz,-gpSz,0),
		vec3( gpSz, gpSz,0),
		vec3(-gpSz, gpSz,0)};
	int quadTop=pvertices.size();
	for(int t=0;t<4;t++){
		pvertices.push_back(vec4(verts[t].x,verts[t].y,verts[t].z,0.0f));
	}	
	int	indices[]={0,1,2,2,3,0};
	for(int t=0;t<6;t++){
		pindices.push_back(quadTop+indices[t]);
	}
#endif

	neTriangleMesh nmesh;
	nmesh.vertices=(neV3*)&pvertices[0];//new neV3[4];
	nmesh.vertexCount=pvertices.size();

	nmesh.triangleCount=pindices.size()/3;
	std::vector<neTriangle>	triangles;
	int ibase=0;
	for(int t=0;t<nmesh.triangleCount;t++){
		neTriangle tri;
		for(int ti=0;ti<3;ti++)tri.indices[ti]=pindices[((t*3)+ti)];//ibase++;
		tri.materialID = 0;
		tri.flag = neTriangle::NE_TRI_TRIANGLE;
		triangles.push_back(tri);
	}
	nmesh.triangles=&triangles[0];

	simulator->SetTerrainMesh(&nmesh);

}

const s32 N_DUDE=100;


class PhysicsListener : public Listener
{
public:
	virtual bool	message(const char *msg=null,void*	params=null){
		if(msg=="engineStep"){
			simulator->Advance(1.0f/60);

			//simulator->Advance(1.0f / 30.0f, 1.0f / 60.0f, 1.0f/ 30.0f, NULL);

		}else if(msg=="renderSprites"){
			
		}else if(msg=="networkDataFromServer"){
			NetworkDGram&	dgram=*(NetworkDGram*)params;
			//logstr(format("Got data from server:%s",params));

		}else if(msg=="networkDataFromClient"){
			NetworkDGram&	dgram=*(NetworkDGram*)params;
			if(strcmp("control",(char*)params)==0){
				//left click...
				Sprite*	spr=Sprite::findSprite(dgram.control.targetSpriteID);
				if(spr){
					//Apply command to target sprite
					if(dgram.control.controlBits & 1){
						logstr(format("Client click...",params));
					}
				}else{
					//Apply command in isolation.
				}
			}else if(strcmp("welcome",(char*)params)){
				logstr(format("got welcome."));
			}else{
				logstr(format("unknown server packet type:%s",(char*)params));
			}
		}else if(msg=="physicsAddRagdoll"){
			physicsAddRagdollParams&	boxdef=*(physicsAddRagdollParams*)params;
			boxdef.outSprite=makeRadDude(simulator,(neV3&)boxdef.position,rotateX3(PI*0.5f),gRagdollStackTop);

		}else if(msg=="physicsAddBox"){
			physicsAddBoxParams&	boxdef=*(physicsAddBoxParams*)params;
			vec3*	bxdef=(vec3*)params;

			neAnimatedBody*	body = simulator->CreateAnimatedBody();

			neGeometry * geom = body->AddGeometry();
			geom->SetBoxSize((neV3&)boxdef.dimensions);

			body->UpdateBoundingInfo();
			body->SetPos((neV3&)boxdef.position);
			boxdef.body=body;
		}else if(msg=="resetCamera"){
			((PhysicsSpriteController*)playerSprite->controller)->body->SetPos((neV3&)vec3(-94.905891f,-266.18021f,1.98850018f));
		}else if(msg=="physicsBuildSprites"){
			playerSprite=makeRigidBodySprite();
			//playerSprite->position=vec3(-94.905891f,-266.18021f,1.98850018f);
			playerSprite->position=vec3(-2.905891f,-2.18021f,1.98850018f);
			((PhysicsSpriteController*)playerSprite->controller)->body->SetPos((neV3&)playerSprite->position);

			gApp->camTargetID=playerSprite->id;
			gApp->cameraMode=App::CAM_TRACK_FPS;
			for(int t=0;t<1;t++){//N_DUDE/4;t++){

				makeRadDude(simulator,(neV3&)vec3(frand()*5.0f,frand()*5.0f,3.0f),rotateX3(PI*0.5f),gRagdollStackTop);
			}
		}else if(msg=="physicsBoot"){

			neSimulatorSizeInfo sizeInfo;
			neV3 gravity; gravity.Set(0.0f, 0.0f,-9.8f);

			sizeInfo.terrainNodesStartCount=10000;
			sizeInfo.geometriesCount = 5000+(64*64);
			sizeInfo.constraintBufferSize *= 100;
			sizeInfo.overlappedPairsCount *= 100;
			sizeInfo.rigidBodiesCount *= 100;
			sizeInfo.constraintsCount *= 100;
			sizeInfo.constraintSetsCount *= 100;
			sizeInfo.animatedBodiesCount = 5000+(64*64);

			simulator = neSimulator::CreateSimulator(sizeInfo, &allocator, &gravity);

			simulator->SetMaterial(0,0.5f,0.5f);
			simulator->SetMaterial(1,0.5f,0.5f);
			simulator->GetCollisionTable()->Set(1,2,neCollisionTable::RESPONSE_IMPULSE_CALLBACK);
			simulator->GetCollisionTable()->Set(0,2,neCollisionTable::RESPONSE_IMPULSE_CALLBACK); //Report ground+ragdoll collisions
			//simulator->GetCollisionTable()->Set(2,1,neCollisionTable::RESPONSE_IMPULSE_CALLBACK);
			simulator->SetCollisionCallback(CollisionCallback);
#if 0
			Sprite*	ground=makeAnimatedBodySprite(null,"tile_wall");
//			neAnimatedBody*	ground = simulator->CreateAnimatedBody();
//			neGeometry * geom = ground->AddGeometry();	 
//			geom->SetBoxSize((neV3&)vec3(1000,1000,100));
//			ground->UpdateBoundingInfo();
//			ground->SetPos((neV3&)vec3(0,0,-50));
		
//			physicsAddRagdollParams	ragdolldef={vec3(0,0,1.0f),null};
//			Listener::sendMessage("physicsAddRagdoll",&ragdolldef);
#endif
		}
		return true;
	}
	PhysicsListener(){
		Listener::listen(this,"physicsBoot");
		Listener::listen(this,"physicsShutdown");
		Listener::listen(this,"physicsBuildSprites");
		Listener::listen(this,"engineStep");
		Listener::listen(this,"renderSprites");
		Listener::listen(this,"resetCamera");
		Listener::listen(this,"physicsAddBox");
		Listener::listen(this,"physicsAddRagdoll");
		Listener::listen(this,"networkDataFromClient");
		Listener::listen(this,"networkDataFromServer");

	}
};

PhysicsListener	gPhysicsListener;